#include "MeStrUtil.h"

#include <QStringList>


QString MeStrUtil::doublesToQStr(double values[], int size)
{
    QString str = "";
    if (size == 0) {
        return str;
    }
    int i;
    for (i=0; i < size-1; i++) {
        str.append(QString::asprintf("%lf, ", values[i]));
    }
    str.append(QString::asprintf("%lf", values[i]));

    return str;
}

void MeStrUtil::doublesFromQStr(QString &str, std::vector<double> &values)
{
    QStringList strPts = str.trimmed().split(",");
    values.clear();
    if (strPts.empty()) {
        return;
    }
    bool ok = false;
    foreach (const QString &oneStr, strPts) {
        double d = oneStr.toDouble(&ok);
        values.push_back(d);
    }
}

QString MeStrUtil::poseToQStr(double values[])
{
    return QString::asprintf("%lf, %lf, %lf, %lf, %lf, %lf", values[0], values[1], values[2], values[3], values[4], values[5]);
}

void MeStrUtil::poseFromQStr(QString &str, double posev[])
{
    sscanf(str.toLatin1(), "%lf, %lf, %lf, %lf, %lf, %lf",
           &posev[0], &posev[1], &posev[2], &posev[3], &posev[4], &posev[5]);
}

void MeStrUtil::poseFromStr(std::string &str, double v[])
{
    sscanf(str.c_str(), "%lf, %lf, %lf, %lf, %lf, %lf",
           &v[0], &v[1], &v[2], &v[3], &v[4], &v[5]);
}

void MeStrUtil::poseFromStr(std::string &str, MePose &pose)
{
    MeStrUtil::poseFromStr(str, pose.fv);
}

void MeStrUtil::jointsFromStr(std::string &str, double jv[])
{
    sscanf(str.c_str(), "%lf, %lf, %lf, %lf, %lf, %lf",
           &jv[0], &jv[1], &jv[2], &jv[3], &jv[4], &jv[5]);
}

void MeStrUtil::jointsFromStr(std::string &str, MeJoints &joints)
{
    MeStrUtil::poseFromStr(str, joints.v);
}

QString MeStrUtil::point3dToQStr(double values[])
{
    return QString::asprintf("%lf, %lf, %lf", values[0], values[1], values[2]);
}

QString MeStrUtil::point3dToQStr(const double values[])
{
    return QString::asprintf("%lf, %lf, %lf", values[0], values[1], values[2]);
}

QString MeStrUtil::point3dToQStr3Prec(double values[])
{
    return QString::asprintf("%0.3lf, %0.3lf, %0.3lf", values[0], values[1], values[2]);
}

QString MeStrUtil::point3dToQStr3Prec(const double values[])
{
    return QString::asprintf("%0.3lf, %0.3lf, %0.3lf", values[0], values[1], values[2]);
}


void MeStrUtil::point3dFromQStr(QString &str, double values[])
{
    sscanf(str.toLatin1(), "%lf, %lf, %lf",
           &values[0], &values[1], &values[2]);
}

void MeStrUtil::point3dFromStr(std::string &str, double values[])
{
    sscanf(str.c_str(), "%lf, %lf, %lf",
           &values[0], &values[1], &values[2]);
}

void MeStrUtil::pointsFromQString(QString &strPts, std::vector<MePoint3D> &pts)
{
    pts.clear();

    QStringList strList = strPts.split("\n");
    MePoint3D point3D;
    for (int i = 0; i< strList.size(); i++) {
        QString &tmp = strList[i];
        if (tmp.size() < 5) { // 最差为 0,0,0
            continue;
        }
        sscanf(tmp.toLatin1(), "%lf, %lf, %lf", &point3D.v[0], &point3D.v[1], &point3D.v[2]);
        pts.push_back(point3D);
    }
}

void MeStrUtil::pointsFromQString(QString &strPts, std::vector<Eigen::Vector3d> &pts)
{
    pts.clear();

    QStringList strList = strPts.split("\n");
    Eigen::Vector3d point3D;
    for (int i = 0; i< strList.size(); i++) {
        QString &tmp = strList[i];
        if (tmp.size() < 5) { // 最差为 0,0,0
            continue;
        }
        sscanf(tmp.toLatin1(), "%lf, %lf, %lf", &point3D[0], &point3D[1], &point3D[2]);
        pts.push_back(point3D);
    }
}

QString MeStrUtil::pointsToQString(const std::vector<MePoint3D> &pts)
{
    QString str;
    for (uint i=0; i< pts.size(); i++) {
        const double *p = pts[i].v;
        str.append(QString::asprintf("%lf, %lf, %lf\n", p[0], p[1], p[2]));
    }
    return str;
}

QString MeStrUtil::pointsToQString(const std::vector<Eigen::Vector3d> &pts, const char *sep)
{
    QString str;
    for (uint i=0; i< pts.size(); i++) {
        const double *p = pts[i].data();
        str.append(QString::asprintf("%lf, %lf, %lf%s", p[0], p[1], p[2], sep));
    }
    return str;
}


#ifndef ME_NO_CV
QString MeStrUtil::point3dToQStr(cv::Point3d &pt)
{
    return QString::asprintf("%lf, %lf, %lf", pt.x, pt.y, pt.z);
}

QString MeStrUtil::point3fToQStr(cv::Point3f &pt)
{
    return QString::asprintf("%f, %f, %f", pt.x, pt.y, pt.z);
}

void MeStrUtil::point3dFromQStr(QString &str, cv::Point3d &pt)
{
    sscanf(str.toLatin1(), "%lf, %lf, %lf",
           &pt.x, &pt.y, &pt.z);
}

QString MeStrUtil::pointsToQString(const std::vector<cv::Point3d> &pts)
{
    QString str;
    for (uint i=0; i< pts.size(); i++) {
        const cv::Point3d &p = pts[i];
        str.append(QString::asprintf("%lf, %lf, %lf\n", p.x, p.y, p.z));
    }
    return str;
}

QString MeStrUtil::pointsToQString(const std::vector<cv::Point3f> &pts)
{
    QString str;
    for (uint i=0; i< pts.size(); i++) {
        const cv::Point3f &p = pts[i];
        str.append(QString::asprintf("%f, %f, %f\n", p.x, p.y, p.z));
    }
    return str;
}

QString MeStrUtil::pointsToQString(const std::vector<cv::Point2f> &pts)
{
    QString str;
    for (uint i=0; i< pts.size(); i++) {
        const cv::Point2f &p = pts[i];
        str.append(QString::asprintf("%f, %f\n", p.x, p.y));
    }
    return str;
}


void MeStrUtil::pointsFromQString(QString &strPts, std::vector<cv::Point3d> &pts)
{
    pts.clear();

    QStringList strList = strPts.split("\n");
    cv::Point3d point3D;
    for (int i = 0; i< strList.size(); i++) {
        QString &tmp = strList[i];
        if (tmp.size() < 5) { // 最差为 0,0,0
            continue;
        }
        sscanf(tmp.toLatin1(), "%lf, %lf, %lf", &point3D.x, &point3D.y, &point3D.z);
        pts.push_back(point3D);
    }
}
#endif // ME_NO_CV
